# transformer.py

r'''The transformer object deals with how to place the work and the tool on
the target CNC machine to duplicate the piece cut on the virtual OT machine.

There are several base classes that each deal with a different technique for
doing this.  These are broken out by how many translation axes and how many
rotation axes the target CNC machine supports.  The translation axes are X, Y
and Z and support basic movement.  The rotation axes are A, B, and C which
each rotate in place around a different axis (A rotates around the X axis, B
around the Y axis, and C around the Z axis).
'''

from __future__ import division

import math
import operator
import sys      # for debugging

from ot_simulator import transform

class transformer(object):
    r'''Base class for all transformers.
    '''
    def __init__(self, cnc_to_work_transform=transform.Identity_transform,
                 transform_funs=(operator.pos, operator.pos, operator.pos)):
        self.cnc_to_work_transform = cnc_to_work_transform

        # Figure out the mapping of axes from work to cnc coordinates.
        #
        # First, get cnc coordinates of work (1,2,3) vector.
        cnc_coord = \
          tuple(round(n, 6) \
                for n
                 in self.cnc_to_work_transform.local_to_global((1.0, 2.0, 3.0)))

        # Load self.work_axis_to_cnc_axis with conversions for XYZ axes.
        self.work_axis_to_cnc_axis = {}
        num_negatives = 0
        for cnc_axis, coord, conv_fn in zip("XYZ", cnc_coord, transform_funs):
            # work_axis is where this came from
            work_axis = "XYZ"[(1.0, 2.0, 3.0).index(abs(coord))]

            if coord < 0.0:
                self.work_axis_to_cnc_axis[work_axis] = \
                  (cnc_axis, lambda n: -conv_fn(n))
                num_negatives += 1
            else:
                self.work_axis_to_cnc_axis[work_axis] = (cnc_axis, conv_fn)

        # Load self.work_axis_to_cnc_axis with conversions for ABC axes.
        rot_lookup = dict(X='A', Y='B', Z='C')
        for work_axis in "XYZ":
            cnc_axis = self.work_axis_to_cnc_axis[work_axis][0]

            # If num_negatives is odd, this is a left-handed coordinate system
            # and the direction of rotations are reversed.
            if num_negatives % 2: convert = operator.neg
            else: convert = operator.pos

            self.work_axis_to_cnc_axis[rot_lookup[work_axis]] = \
              (rot_lookup[cnc_axis], convert)

        #print >> sys.stderr, "work_axis_to_cnc_axis", \
        #                     self.work_axis_to_cnc_axis

    def convert_pos(self, pos):
        r'''Convert a position from work to cnc coordinates.

        The input and output position are strings with coordinates separated
        by spaces, as in G-Code.
        '''
        new_positions = {}   # {axis: value}
        for p in pos.split():
            work_axis, value = p[0], float(p[1:])
            cnc_axis, conv_fn = self.work_axis_to_cnc_axis[work_axis.upper()]
            new_positions[cnc_axis] = conv_fn(value)
        return new_positions

class transform_3t_0r(transformer):
    r'''This handles the conversion to a 3-axis mill with no rotation axes.

    The ``cnc_to_work_transform`` is only allowed to be a rotation transform.
    It must not scale or translate the axes.

    But, since there are no rotations, we can allow translations on the axes;
    but these are specified separately.

    The way that this transform is done is to simply drop the work on the CNC
    machine and see where the tool tip lands in X/Y/Z.  In other words, the
    lathe to work transform is always simply the ``cnc_to_work_transform``
    plus the translations on each axis.  The attitude (orientation) of the tool
    is ignored, as these machines do not allow rotations.
    '''
    def __init__(self, cnc_to_work_transform=transform.Identity_transform,
                 x_offset=0.0, y_offset=0.0, z_offset=0.0):
        transformer.__init__(self, cnc_to_work_transform,
                             (lambda x: x + x_offset,
                              lambda y: y + y_offset,
                              lambda z: z + z_offset))
        self.x_offset = x_offset
        self.y_offset = y_offset
        self.z_offset = z_offset
        pre_work_transform = transform.transform.translation(self.x_offset,
                                                             self.y_offset,
                                                             self.z_offset)
        self.cnc_to_work_transform = \
          pre_work_transform.add(self.cnc_to_work_transform)

    def convert(self, work_to_tool_transform):
        r'''Returns tool_transform.
        '''
        tool_transform = self.cnc_to_work_transform.add(work_to_tool_transform)
        x, y, z = tool_transform.local_to_global()
        return dict(X=x, Y=y, Z=z)

class transform_3t_1r(transformer):
    r'''This handles the conversion to a 4-axis machine.

    The ``cnc_to_work_transform`` is only allowed to be a rotation transform.
    It must not scale or translate the axes.

    The way that this is done is to rotate the work so that the tool shank
    lies vertically on a plane parallel to both the ``cnc_rotation_axis`` and 
    the ``cnc_tool_axis`` with a positive ``cnc_tool_axis``.  Then the
    position of the tool tip is taken as the X/Y/Z position.

    This results in one axis of the tool's attitude being set, and the other
    two axes ignored.
    '''
    def __init__(self, cnc_to_work_transform=transform.Identity_transform,
                 cnc_rotation_axis="C", cnc_tool_axis="X"):
        transformer.__init__(self, cnc_to_work_transform)
        if len(cnc_rotation_axis) != 1 or cnc_rotation_axis not in "ABC":
            raise ValueError("Illegal cnc_rotation_axis: {0!r}"
                               .format(cnc_rotation_axis))
        if len(cnc_tool_axis) != 1 or cnc_tool_axis not in "XYZ":
            raise ValueError("Illegal cnc_tool_axis: {0!r}"
                               .format(cnc_tool_axis))
        if cnc_rotation_axis == Axis_convert[cnc_tool_axis]:
            raise ValueError("cnc_tool_axis, {0!r}, must not be the same as "
                             "the cnc_rotation_axis, {1!r}"
                               .format(cnc_tool_axis, cnc_rotation_axis))
        self.cnc_rotation_axis = cnc_rotation_axis
        self.cnc_tool_axis = cnc_tool_axis

    def convert(self, work_to_tool_transform):
        r'''Returns dict of {axis: value} for position to go to.
        '''
        tool_transform = self.cnc_to_work_transform.add(work_to_tool_transform)

        # position of tool tip (offset 0 on tool Z axis):
        x0, y0, z0 = tool_transform.local_to_global()

        # position 1" up the tool shank (tool Z axis):
        x1, y1, z1 = tool_transform.local_to_global((0.0, 0.0, 1.0))

        x, y, z = x1 - x0, y1 - y0, z1 - z0
        if self.cnc_rotation_axis == 'A':
            if self.cnc_tool_axis == 'Y':
                work_angle = -math.degrees(math.atan2(z, y))
            else:
                work_angle = math.degrees(math.atan2(y, z))
            rotation = transform.transform.rotation_on_x(work_angle)
        elif self.cnc_rotation_axis == 'B':
            if self.cnc_tool_axis == 'X':
                work_angle = math.degrees(math.atan2(z, x))
            else:
                work_angle = -math.degrees(math.atan2(x, z))
            rotation = transform.transform.rotation_on_y(work_angle)
        else: # self.cnc_rotation_axis == 'C'
            if self.cnc_tool_axis == 'X':
                work_angle = -math.degrees(math.atan2(y, x))
            else:
                work_angle = math.degrees(math.atan2(x, y))
            rotation = transform.transform.rotation_on_z(work_angle)
        new_positions = \
          dict(zip("XYZ", rotation.add(tool_transform).local_to_global()))
        new_positions[self.cnc_rotation_axis] = work_angle
        return new_positions

class transform_2t_1r(transformer):
    r'''This handles the conversion to a 3-axis lathe with one rotation axis
    and two translation axes.

    The ``cnc_to_work_transform`` is only allowed to be a rotation transform.
    It must not scale or translate the axes.

    The way that this is done is to rotate the work so that the tool tip is at
    zero on the missing X/Y/Z axis.  Then if the ``cnc_tool_vertical_axis`` is
    upside down, the work rotated an additional 180 degrees.  Finally, the
    postition of the tool tip on the remaining two axes is taken.  The attitude
    (orientation) of the tool is ignored, as the one rotation that these
    machines allow must be used to compensate for the lack of a third
    translation axis.
    '''
    def __init__(self, cnc_to_work_transform=transform.Identity_transform,
                 cnc_rotation_axis="C", cnc_missing_translation_axis="Y",
                 cnc_tool_vertical_axis="+Y"):
        transformer.__init__(self, cnc_to_work_transform)
        if len(cnc_rotation_axis) != 1 or cnc_rotation_axis not in "ABC":
            raise ValueError("Illegal cnc_rotation_axis: {0!r}"
                               .format(cnc_rotation_axis))
        if len(cnc_missing_translation_axis) != 1 \
           or cnc_missing_translation_axis not in "XYZ":
            raise ValueError(
                    "Illegal cnc_missing_translation_axis: {0!r}"
                      .format(cnc_missing_translation_axis))
        if Axis_convert[cnc_missing_translation_axis] == cnc_rotation_axis:
            raise ValueError(
                    "cnc_missing_translation_axis, {0!r}, must not be the "
                    "same as the cnc_rotation_axis, {1!r}"
                      .format(cnc_missing_translation_axis, cnc_rotation_axis))
        self.cnc_rotation_axis = cnc_rotation_axis
        self.cnc_missing_translation_axis = cnc_missing_translation_axis
        self.set_cnc_tool_vertical_axis(cnc_tool_vertical_axis)

    def set_cnc_tool_vertical_axis(self, cnc_tool_vertical_axis):
        if len(cnc_tool_vertical_axis) != 2 \
           or cnc_tool_vertical_axis[0] not in "+-" \
           or cnc_tool_vertical_axis[1] not in "XYZ" \
           or Axis_convert[cnc_tool_vertical_axis[1]] == self.cnc_rotation_axis:
            raise ValueError("Illegal cnc_tool_vertical_axis: {0!r}"
                               .format(cnc_missing_translation_axis))
        self.cnc_tool_vertical_axis = cnc_tool_vertical_axis[1]
        offset = 1 if cnc_tool_vertical_axis[0] == '+' else -1
        if self.cnc_tool_vertical_axis == 'X':
            self.tool_test_offset = (offset, 0.0, 0.0)
        elif self.cnc_tool_vertical_axis == 'Y':
            self.tool_test_offset = (0.0, offset, 0.0)
        else:
            self.tool_test_offset = (0.0, 0.0, offset)

    def convert(self, work_to_tool_transform):
        r'''Returns dict of {axis: value} for position to go to.
        '''
        tool_transform = self.cnc_to_work_transform.add(work_to_tool_transform)

        # position of tool tip (offset 0 on tool Z axis):
        x, y, z = tool_transform.local_to_global()

        if self.cnc_rotation_axis == 'A':
            if self.cnc_missing_translation_axis == 'Y':
                work_angle = math.degrees(math.atan2(y, z))
            else:
                work_angle = -math.degrees(math.atan2(z, y))
            rotation = transform.transform.rotation_on_x(work_angle)
        elif self.cnc_rotation_axis == 'B':
            if self.cnc_missing_translation_axis == 'X':
                work_angle = -math.degrees(math.atan2(x, z))
            else:
                work_angle = math.degrees(math.atan2(z, x))
            rotation = transform.transform.rotation_on_y(work_angle)
        else: # self.cnc_rotation_axis == 'C'
            if self.cnc_missing_translation_axis == 'X':
                work_angle = math.degrees(math.atan2(x, y))
            else:
                work_angle = -math.degrees(math.atan2(y, x))
            rotation = transform.transform.rotation_on_z(work_angle)

        rotated_tool_transform = rotation.add(tool_transform)

        x0, y0, z0 = rotated_tool_transform.local_to_global()
        x1, y1, z1 = rotated_tool_transform.local_to_global(
                       self.tool_test_offset)
        deltas = dict(zip("XYZ", (x1 - x0, y1 - y0, z1 - z0)))
        if deltas[self.cnc_tool_vertical_axis] < 0.0:
            work_angle += 180
            if self.cnc_rotation_axis == 'A':
                rotation = transform.transform.rotation_on_x(work_angle)
            elif self.cnc_rotation_axis == 'B':
                rotation = transform.transform.rotation_on_y(work_angle)
            else: # self.cnc_rotation_axis == 'C'
                rotation = transform.transform.rotation_on_z(work_angle)
            rotated_tool_transform = rotation.add(tool_transform)

        new_positions = \
          dict(zip("XYZ", rotated_tool_transform.local_to_global()))
        new_positions[self.cnc_rotation_axis] = work_angle
        assert abs(new_positions[self.cnc_missing_translation_axis]) < 1e-4, \
          "failed to get {0} axis to zero" \
            .format(self.cnc_missing_translation_axis)
        return new_positions

Axis_convert = dict(zip("XYZ", "ABC"))

